import pandas as pd
import numpy as np


def read_and_interpolate_csv(file_path):
    # 读取CSV文件
    df = pd.read_csv(file_path)

    # 初始化traces列表
    traces = []

    # 遍历每一行，计算需要插入的点
    for i in range(len(df) - 1):
        start_th = df.iloc[i]['th_smooth']
        end_th = df.iloc[i + 1]['th_smooth']
        start_ph = df.iloc[i]['ph_smooth']
        end_ph = df.iloc[i + 1]['ph_smooth']

        # 计算需要插入的点的数量
        num_points = int(max(abs(end_th - start_th), abs(end_ph - start_ph)))

        if num_points > 0:
            th_values = np.linspace(start_th, end_th, num_points + 1)
            ph_values = np.linspace(start_ph, end_ph, num_points + 1)

            for j in range(num_points + 1):
                traces.append([th_values[j], ph_values[j]])
        else:
            traces.append([start_th, start_ph])

    # 添加最后一个点
    traces.append([df.iloc[-1]['th_smooth'], df.iloc[-1]['ph_smooth']])

    return traces


def interp_th_ph(path_csv, path_save):
    traces = read_and_interpolate_csv(path_csv)
    print(f"len of traces={len(traces)}")
    # 将traces转换为DataFrame
    interpolated_df = pd.DataFrame(traces, columns=['th_smooth', 'ph_smooth'])
    # 保存到新的CSV文件
    interpolated_df.to_csv(path_save, index=False)


if __name__=="__main__":
    interp_th_ph("../files/archive/uav_dataset_xy_thph.csv", "../files/archive/uav_dataset_thph_interp.csv")


